/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-01-15     Wangjiangbo       the first version
 */
#include <app/app_arm_control.h>
#include <App/app_io.h>

#define DBG_TAG "arm"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

arm_direct  _arm_direct ={-1,-1,-1,-1,-1,-1,-1};

enum ARM_STATE {
    TO_ARM_DISABLE = 0,
    ARM_DISABLE,
    TO_ARM_ENABLE,
    ARM_ENABLE,
};

enum ARM_RUN_CMD {
    CMD_NONE = 0,
    CMD_ENABLE = 1,
    CMD_DISABLE,
    CMD_UP_DOWN,
    CMD_LEFT_RIGHT,
    CMD_IN_OUT,
    CMD_PITCH,
    CMD_YAW,
    CMD_ROLL,
    CMD_UNFOLD_STORAGE,
};


/* 邮箱控制块 */
static struct rt_mailbox mb;
/* 用于放邮件的内存池 */
static char mb_pool[128];

static void __all_disable(void)
{
    memset(&_arm_direct,-1,sizeof(arm_direct));
    LOG_D("机械臂调整关闭");
}

static int32_t __msg_recv(int32_t* p_msg, int32_t* p_paramter, int32_t timeout)
{
    int32_t msg = 0;
    int32_t result = 0;
    result = rt_mb_recv(&mb, (rt_ubase_t *)&msg, timeout);
    *p_msg = msg & 0xffff;
    *p_paramter = (msg >> 16) & 0xffff;
    return result;
}

// 线程入口
static void thread_entry(void* parameter)
{
    int32_t motor_cmd = 0;
    int32_t motor_param = 0;
    int8_t state = TO_ARM_DISABLE;

    rt_thread_mdelay(1000);

    while(1)
    {
        switch(state)
        {
        case TO_ARM_DISABLE:
            __all_disable();
            state = ARM_DISABLE;
            break;

        case ARM_DISABLE:
            __msg_recv(&motor_cmd, &motor_param, RT_WAITING_FOREVER);
            if(motor_cmd == CMD_ENABLE)
                state = TO_ARM_ENABLE;
            break;

        case TO_ARM_ENABLE:
            state = ARM_ENABLE;
            LOG_D("ARM使能");
            break;

        case ARM_ENABLE:
        {
            int32_t r = __msg_recv(&motor_cmd, &motor_param, 300);
            int16_t motor_vel = motor_param & 0x00ffff;
            if(r == RT_EOK)
            {
                if(motor_cmd == CMD_DISABLE)
                    state = TO_ARM_DISABLE;

                if(motor_cmd == CMD_UP_DOWN)
                {
                    _arm_direct.u_d = (int8_t)motor_vel;
                }
                else if(motor_cmd == CMD_LEFT_RIGHT)
                {
                    _arm_direct.l_r = (int8_t)motor_vel;
                }
                else if(motor_cmd == CMD_IN_OUT)
                {
                    _arm_direct.i_o = (int8_t)motor_vel;
                }
                if(motor_cmd == CMD_PITCH)
                {
                    _arm_direct.pitch = (int8_t)motor_vel;
                }
                else if(motor_cmd == CMD_YAW)
                {
                    _arm_direct.yaw = (int8_t)motor_vel;
                }
                else if(motor_cmd == CMD_ROLL)
                {
                    _arm_direct.roll = (int8_t)motor_vel;
                }
                else if(motor_cmd == CMD_UNFOLD_STORAGE)
                {
                    _arm_direct.uf_st = (int8_t)motor_vel;
                }
            }
            if( r == -RT_ETIMEOUT)
            {
                memset(&_arm_direct,-1,sizeof(arm_direct));
            }
            if(motor_cmd == CMD_DISABLE)
                state = TO_ARM_DISABLE;

            break;
        }
        default:
           break;
        }

    }
}

void app_arm_init(void)
{
    #define THREAD_STACK_SIZE   1024
    #define THREAD_PRIORITY     8
    #define THREAD_TIMESLICE    10

    rt_thread_t tid = RT_NULL;

    rt_err_t result;

    /* 初始化一个 mailbox */
    result = rt_mb_init(&mb,
                        "arm_mbt",                      /* 名称是 mbt */
                        &mb_pool[0],                /* 邮箱用到的内存池是 mb_pool */
                        sizeof(mb_pool) / 4,        /* 邮箱中的邮件数目，因为一封邮件占 4 字节 */
                        RT_IPC_FLAG_FIFO);          /* 采用 FIFO 方式进行线程等待 */
    // 创建线程
    tid = rt_thread_create("arm control",
                            thread_entry, RT_NULL,
                            THREAD_STACK_SIZE,
                            THREAD_PRIORITY, THREAD_TIMESLICE);



    if (tid != RT_NULL)
        rt_thread_startup(tid);

}

void app_arm_enable(void)
{
    rt_err_t result;
    result = rt_mb_send(&mb, CMD_ENABLE);
    LOG_D("请求arm调整开启");
}

void app_arm_disable(void)
{
    rt_err_t result;
    result = rt_mb_send(&mb, CMD_DISABLE);
    LOG_D("请求arm调整关闭");
}

void app_arm_run(int direct, int32_t vel)
{
    rt_err_t result;
    if(direct == UP_DOWN)
    {
        int32_t val = ((vel << 16) | CMD_UP_DOWN);
        result = rt_mb_send(&mb, val);
    }
    else if (direct == LEFT_RIGHT)
    {
        int32_t val = ((vel << 16) | CMD_LEFT_RIGHT);
        result = rt_mb_send(&mb, val);
    }
    else if (direct == IN_OUT)
    {
        int32_t val = ((vel << 16) | CMD_IN_OUT);
        result = rt_mb_send(&mb, val);
    }
    if(direct == PITCH)
    {
        int32_t val = ((vel << 16) | CMD_PITCH);
        result = rt_mb_send(&mb, val);
    }
    else if (direct == YAW)
    {
        int32_t val = ((vel << 16) | CMD_YAW);
        result = rt_mb_send(&mb, val);
    }
    else if (direct == ROLL)
    {
        int32_t val = ((vel << 16) | CMD_ROLL);
        result = rt_mb_send(&mb, val);
    }
    else if (direct == UNFOLD_STORAGE)
    {
        int32_t val = ((vel << 16) | CMD_UNFOLD_STORAGE);
        result = rt_mb_send(&mb, val);
    }
}


void app_get_arm_direct(arm_direct *_direct)
{
    rt_enter_critical();
    memcpy(_direct, &_arm_direct, sizeof(arm_direct));
    rt_exit_critical();
}
